#include "cline3d.h"

/**
 * CLine3D - creates line identical with x-axis
 */
template <typename T>
CLine3D<T>::CLine3D() {
  CVector3D<T> tmpPoint(0, 0, 0), tmpDirection(1, 0, 0);
  set(tmpPoint, tmpDirection);
}
//---------------------------------------------------------------------------

/**
 * CLine3D - copy constructor (deep copy)
 * @param l
 */
template <typename T>
CLine3D<T>::CLine3D(const CLine3D<T> &l) {
  this->copyFrom(l);
}
//---------------------------------------------------------------------------

/**
 * CLine3D - creates line from two given points
 * @param p1
 * @param p2
 */
template <typename T>
CLine3D<T>::CLine3D(const CVector3D<T> &p1, const CVector3D<T> &p2) {
  set(p1, p2 - p1);
}
//---------------------------------------------------------------------------

/**
 * operator== - compares two lines
 * @param l
 * @return true if lines are identical
 */
template <typename T>
bool CLine3D<T>::operator == (const CLine3D<T> &l) const {

  // Directions must be paralell
  if (!isParalell(l)) return false;

  // Coordinates of points must be in same ratio (vectors origin-point must be
  // LD)
  T t(point.get(0) / l.getPoint().get(0));
  for (TIndex i = 1; i < point.getSize(); i++) {
    if (!CMath<T>::equals(point.get(i) / l.getPoint().get(i), t)) return false;
  }
  return true;
}
//---------------------------------------------------------------------------

/**
 * operator!= - compares two lines
 * @param l
 * @return true if lines are not identical
 */
template <typename T>
bool CLine3D<T>::operator != (const CLine3D<T> &l) const {
  return !(*this == l);
}
//---------------------------------------------------------------------------

/**
 * set - defines line with its point and direction
 * @param p
 * @param direction
 */
template <typename T>
void CLine3D<T>::set(const CVector3D<T> &p, const CVector3D<T> &direction) {
  this->point = p;
  this->direction = direction;
  this->direction.normalize();
}
//---------------------------------------------------------------------------

/**
 * getDirection - returns direction of line
 * @return line direction
 */
template <typename T>
CVector3D<T> CLine3D<T>::getDirection() const {
  return direction;
}
//---------------------------------------------------------------------------

/**
 * getPoint - returns point which defines line (lies on it)
 * @return point which lies on line
 */
template <typename T>
CVector3D<T> CLine3D<T>::getPoint() const {
  return point;
}
//---------------------------------------------------------------------------

/**
 * lineBetween - finds transversal (line between) this line and given line
 * @param l
 * @return transversal
 */
template <typename T>
CLine3D<T> CLine3D<T>::lineBetween(const CLine3D<T> &l) const {

  // Transversal
  //-------------------------------------------------------------------------
  // Transversal must be perpendicular to both lines (vector):
  // x1 = p1 + v1 * t1
  // x2 = p2 + v2 * t2
  //-------------------------------------------------------------------------
  // => :
  //-------------------------------------------------------------------------
  // dot(x2 - x1, v1) = 0
  // dot(x2 - x1, v2) = 0
  //-------------------------------------------------------------------------
  // after substitution:
  //-------------------------------------------------------------------------
  // dot(p2 - p1 + v2 * t2 - v1 * t1, v1) = 0
  // dot(p2 - p1 + v2 * t2 - v1 * t1, v2) = 0
  //-------------------------------------------------------------------------
  // after adjustment and substitution v0 as p2 - p1:
  //-------------------------------------------------------------------------
  // dot(v0, v1) + dot(v2, v1) * t2 - dot(v1, v1) * t1 = 0
  // dot(v0, v2) + dot(v2, v2) * t2 - dot(v1, v2) * t1 = 0
  //-------------------------------------------------------------------------

  // Substitution
  CVector3D<T> v0(l.getPoint() - point);
  CVector3D<T> v1(direction);
  CVector3D<T> v2(l.getDirection());

  // Calculate dot products
  T d01(CVector3D<T>::dot(v0, v1)), d02(CVector3D<T>::dot(v0, v2));
  T d11(CVector3D<T>::dot(v1, v1)), d12(CVector3D<T>::dot(v1, v2));
  T d22(CVector3D<T>::dot(v2, v2));

  // Denominator from t1 equation
  T denom(d22 * d11 - d12 * d12);

  // If denominator is zero, transversal does not exist
  if (CMath<T>::equals(denom, 0)) {
    throw CLineNotExistException(getClassName(), "lineBetween");
  }

  // Calculate t1, t2
  T t1((d01 * d22 - d02 * d12) / denom);
  T t2((d12 * t1 - d02) / d22);

  // Substitution for t1, t2 into line equations, calculate edges of
  // transversal
  CVector3D<T> x1(point + direction * t1);
  CVector3D<T> x2(l.getPoint() + l.getDirection() * t2);

  return CLine3D<T>(x1, x2 - x1);
}
//---------------------------------------------------------------------------

/**
 * pointDist - calculates distance between line and given point
 * @param p
 * @return distance between line and point
 */
template <typename T>
T CLine3D<T>::pointDist(const CVector3D<T> &p) const {

  // Point and line
  //-------------------------------------------------------------------------
  // Line segment that connects given point and closest point on given line
  // must be perpendicular to given line: dot(x - p, direction) = 0
  //-------------------------------------------------------------------------
  // after substitution: dot(point - p + direction * t, direction) = 0
  //-------------------------------------------------------------------------
  // after adjustment and substitution (u = point - p):
  // dot(u, direction) + dot(direction, direction) * t = 0
  //-------------------------------------------------------------------------

  // Calculate "t"
  CVector3D<T> u(point - p);
  T t(-(CVector3D<T>::dot(u, direction)) /
      (CVector3D<T>::dot(direction, direction)));

  // Perpendicular projection of "p" to given line (closest point on line)
  CVector3D<T> x(point + direction * t);

  // Length of line segment, which connects point and line
  return (x - p).getNorm();
}
//---------------------------------------------------------------------------

/**
 * lineDist - calculates distance between two lines
 * @param l
 * @return distance between two lines
 */
template <typename T>
T CLine3D<T>::lineDist(const CLine3D<T> &l) const {
  return lineBetween(l).getDirection().getNorm();
}
//---------------------------------------------------------------------------

/**
 * isPointOnLine - checks whether point lies on line
 * @param p
 * @return true if point "p" lies on line
 */
template <typename T>
bool CLine3D<T>::isPointOnLine(const CVector3D<T> &p) const {

  // Calculates parameter "t" from parametric representation of line
  // (p = point + distance * t)
  T t((p.get(0) - point.get(0)) / direction.get(0));

  // Go through all equations of parametric representation (one equation per
  // coordinate) and test whether "t" fulfils equation, if not point does not
  // lie on line
  for (TIndex i = 1; i < point.getSize(); i++) {
    if (!CMath<T>::equals(p.get(i), point.get(i) + t * direction.get(i))) {
      return false;
    }
  }
  return true;
}
//---------------------------------------------------------------------------

/**
 * isParalell - checks whether are two lines parallel
 * @param l
 * @return true if lines are parallel
 */
template <typename T>
bool CLine3D<T>::isParalell(const CLine3D<T> &l) const {

  // Calculates ration of first corresponding coordinates of given lines
  T t(l.getDirection().get(0) / direction.get(0));

  // If all corresponding coordinates are in same ratio, lines are parallel
  for (TIndex i = 1; i < point.getSize(); i++) {
    if (!CMath<T>::equals(t, l.getDirection().get(i) / direction.get(i))) {
      return false;
    }
  }
  return true;
}
//---------------------------------------------------------------------------

/**
 * isIntersecting - test if two lines are intersecting
 * @param l
 * @param epsilon tolerance for minimal distance of lines
 * @return true if lines are intersecting
 */
template <typename T>
bool CLine3D<T>::isIntersecting(const CLine3D<T> &l, T epsilon) const {

  // If lines are parallel, they are not intersecting, return
  if (isParalell(l)) return false;

  // The possibility that two lines in space are intersecting is very small
  // (only small deflexion causes that lines will never intersect)
  // => instead of calculating intersection we'll find shortest distance
  // between given lines, if this distance is lesser than epsilon then lines
  // are intersecting
  if (lineDist(l) < epsilon) return true;
  else return false;
}
//---------------------------------------------------------------------------

/**
 * isIntersecting - test if two lines are intersecting
 * @param l
 * @return true if lines are intersecting
 */
template <typename T>
bool CLine3D<T>::isIntersecting(const CLine3D<T> &l) const {
  return isIntersecting(l, CMath<T>::EPSILON);
}
//---------------------------------------------------------------------------

/**
 * copyFrom - replace this line with copy of given line
 * @param l
 */
template <typename T>
void CLine3D<T>::copyFrom(const CLine3D<T> &l) {
  this->direction = l.getDirection();
  this->point = l.getPoint();
}
//---------------------------------------------------------------------------

template class CLine3D<float>;
template class CLine3D<double>;
